///////////////////////////////////////////////////////////////////////////
//                                                                       //
//  Class: CDualQuaternion                                               //
//                                                                       //
//  CDualQuaternion to represent rotations and translation.  Each        //
//  component of the dual quaternion is stored as a floating point.      //
//                                                                       //
///////////////////////////////////////////////////////////////////////////

#pragma once

#include "mathlib.h"
#include "Quaternion.h"

template<class FT>
class CDualQuat
{
public:
	CDualQuat(void) {}
	// input: unit quaternion 'q', translation vector 't'
	CDualQuat(const CQuaternion &q, const Vector3 &t)
	{
		// non-dual part (just copy q):
		for (int i=0; i<4; i++) d[0][i] = q[i];
		// dual part:
		d[1][0] = -0.5*(t[0]*q[1] + t[1]*q[2] + t[2]*q[3]);
		d[1][1] = 0.5*( t[0]*q[0] + t[1]*q[3] - t[2]*q[2]);
		d[1][2] = 0.5*(-t[0]*q[3] + t[1]*q[0] + t[2]*q[1]);
		d[1][3] = 0.5*( t[0]*q[2] - t[1]*q[1] + t[2]*q[0]);
	}
	~CDualQuat(void) {}

	// Binary operators
	friend CDualQuat operator+(const CDualQuat& u, const CDualQuat& v)
	{
		CDualQuat dq;
		for (int i=0; i<4; i++)
		{
			dq.d[0][i] = u.d[0][i] + v.d[0][i];
			dq.d[1][i] = u.d[1][i] + v.d[1][i];
		}
		return dq;
	}
	friend CDualQuat operator-(const CDualQuat& u, const CDualQuat& v)
	{
		CDualQuat dq;
		for (int i=0; i<4; i++)
		{
			dq.d[0][i] = u.d[0][i] - v.d[0][i];
			dq.d[1][i] = u.d[1][i] - v.d[1][i];
		}
		return dq;
	}
	friend CDualQuat operator*(const FT s, const CDualQuat& u)
	{
		CDualQuat dq;
		for (int i=0; i<4; i++)
		{
			dq.d[0][i] = u.d[0][i] * s;
			dq.d[1][i] = u.d[1][i] * s;
		}
		return dq;
	}
	friend CDualQuat operator*(const CDualQuat& u, const FT s)
	{
		return s * u;
	}
	CDualQuat& operator+=(const CDualQuat& rd)
	{
		for (int i=0; i<4; i++)
		{
			d[0][i] += rd.d[0][i];
			d[1][i] += rd.d[1][i];
		}
		return *this;
	}
	CDualQuat& operator-=(const CDualQuat& rd)
	{
		for (int i=0; i<4; i++)
		{
			d[0][i] -= rd.d[0][i];
			d[1][i] -= rd.d[1][i];
		}
		return *this;
	}

	void GetQuat(CQuaternion &q, Vector3 &t)
	{
		// regular quaternion (just copy the non-dual part):
		for (int i=0; i<4; i++) q[i] = d[0][i];
		// translation vector:
		t[0] = 2.0*(-d[1][0]*d[0][1] + d[1][1]*d[0][0] - d[1][2]*d[0][3] + d[1][3]*d[0][2]);
		t[1] = 2.0*(-d[1][0]*d[0][2] + d[1][1]*d[0][3] + d[1][2]*d[0][0] - d[1][3]*d[0][1]);
		t[2] = 2.0*(-d[1][0]*d[0][3] - d[1][1]*d[0][2] + d[1][2]*d[0][1] + d[1][3]*d[0][0]);
	}

	void SetTransform(const Matrix3 &R, const Vector3 &t)
	{
		// non-dual part (compute quaternion):
		FT trace = R.M[0] + R.M[4] + R.M[8] + 1.0;
		if (trace > ML_D_HIGH_TOLERANCE) {
			FT s = 0.5 / sqrt(trace);
			d[0][0] = 0.25 / s;						// w0
			d[0][1] = (R.M[7] - R.M[5]) * s;		// x0
			d[0][2] = (R.M[2] - R.M[6]) * s;		// y0
			d[0][3] = (R.M[3] - R.M[1]) * s;		// z0
		} else {
			if (R.M[0]>R.M[4] && R.M[0]>R.M[8]) {
				FT s = 0.5 / sqrt(1.0 + R.M[0] - R.M[4] - R.M[8]);
				d[0][0] = (R.M[5] - R.M[7]) * s;	// w0
				d[0][1] = 0.25 / s;					// x0
				d[0][2] = (R.M[1] + R.M[3]) * s;	// y0
				d[0][3] = (R.M[2] + R.M[6]) * s;	// z0
			} else if (R.M[4] > R.M[8]) {
				FT s = 0.5 / sqrt(1.0 + R.M[4] - R.M[0] - R.M[8]);
				d[0][0] = (R.M[2] - R.M[6]) * s;	// w0
				d[0][1] = (R.M[1] + R.M[3]) * s;	// x0
				d[0][2] = 0.25 / s;					// y0
				d[0][3] = (R.M[5] + R.M[7]) * s;	// z0
			} else {
				FT s = 0.5 / sqrt(1.0 + R.M[8] - R.M[0] - R.M[4]);
				d[0][0] = (R.M[1] - R.M[3]) * s;	// w0
				d[0][1] = (R.M[2] + R.M[6]) * s;	// x0
				d[0][2] = (R.M[5] + R.M[7]) * s;	// y0
				d[0][3] = 0.25 / s;					// z0
			}
		}
		// dual part:
		d[1][0] = -0.5*(t[0]*d[0][1] + t[1]*d[0][2] + t[2]*d[0][3]);	// we
		d[1][1] = 0.5*( t[0]*d[0][0] + t[1]*d[0][3] - t[2]*d[0][2]);	// xe
		d[1][2] = 0.5*(-t[0]*d[0][3] + t[1]*d[0][0] + t[2]*d[0][1]);	// ye
		d[1][3] = 0.5*( t[0]*d[0][2] - t[1]*d[0][1] + t[2]*d[0][0]);	// ze
	}

	void GetTransform(Matrix3 &R, Vector3 &t)
	{
		// 3x3 rotation matrix:
		FT Nq = (d[0][0]*d[0][0] + d[0][1]*d[0][1] + d[0][2]*d[0][2] + d[0][3]*d[0][3]);
		FT c  = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
		FT xc = d[0][1]*c,		yc = d[0][2]*c,		zc = d[0][3]*c;
		FT wx = d[0][0]*xc,		wy = d[0][0]*yc,	wz = d[0][0]*zc;
		FT xx = d[0][1]*xc,		xy = d[0][1]*yc,	xz = d[0][1]*zc;
		FT yy = d[0][2]*yc,		yz = d[0][2]*zc,	zz = d[0][3]*zc;
		R.M[0]	= 1.0 - yy - zz;
		R.M[1]	= xy - wz;
		R.M[2]	= xz + wy;
		R.M[3]	= xy + wz;
		R.M[4]	= 1.0 - xx - zz;
		R.M[5]	= yz - wx;
		R.M[6]	= xz - wy;
		R.M[7]	= yz + wx;
		R.M[8]	= 1.0 - xx - yy;
		// translation vector:
		FT wc = d[0][0]*c;
		t[0] = -d[1][0]*xc + d[1][1]*wc - d[1][2]*zc + d[1][3]*yc;
		t[1] = -d[1][0]*yc + d[1][1]*zc + d[1][2]*wc - d[1][3]*xc;
		t[2] = -d[1][0]*zc - d[1][1]*yc + d[1][2]*xc + d[1][3]*wc;
	}

	void GetMatrix(Matrix4 &Mat)
	{
		// 3x3 rotation matrix:
		FT Nq = (d[0][0]*d[0][0] + d[0][1]*d[0][1] + d[0][2]*d[0][2] + d[0][3]*d[0][3]);
		FT c  = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
		FT xc = d[0][1]*c,		yc = d[0][2]*c,		zc = d[0][3]*c;
		FT sx = d[0][0]*xc,		sy = d[0][0]*yc,	sz = d[0][0]*zc;
		FT xx = d[0][1]*xc,		xy = d[0][1]*yc,	xz = d[0][1]*zc;
		FT yy = d[0][2]*yc,		yz = d[0][2]*zc,	zz = d[0][3]*zc;
		Mat.M[0]	= 1.0 - (yy + zz);
		Mat.M[1]	= xy - sz;
		Mat.M[2]	= xz + sy;
		Mat.M[4]	= xy + sz;
		Mat.M[5]	= 1.0 - (xx + zz);
		Mat.M[6]	= yz - sx;
		Mat.M[8]	= xz - sy;
		Mat.M[9]	= yz + sx;
		Mat.M[10]	= 1.0 - (xx + yy);
		// translation vector:
		FT wc = d[0][0]*c;
		Mat.M[3] = -d[1][0]*xc + d[1][1]*wc - d[1][2]*zc + d[1][3]*yc;
		Mat.M[7] = -d[1][0]*yc + d[1][1]*zc + d[1][2]*wc - d[1][3]*xc;
		Mat.M[11] = -d[1][0]*zc - d[1][1]*yc + d[1][2]*xc + d[1][3]*wc;
		Mat.M[12]	= Mat.M[13] = Mat.M[14] = 0.0;
		Mat.M[15]	= 1.0;
	}

private:
	FT d[2][4];	// data
};
